/**
 * @file NewProtocolCurtisRuiBoTe.cpp
 * @author SoBigDog (623644973lubin@163.com)
 * @brief 
 * @version 0.1
 * @date 2020-04-14
 * 
 * @copyright Copyright (c) 2020
 * 
 */
#include "NewProtocolCurtisRuiBoTe.h"
#include "Console.h"
#include "MessageTask.h"
#include "GlobalData.h"
#include "stdio.h"

int16_t CNewProtocolCurtisRuiBoTe::_speedVal = 0;
int16_t CNewProtocolCurtisRuiBoTe::_forkVel = 0;
int16_t CNewProtocolCurtisRuiBoTe::_lower_flag = 0;
int16_t CNewProtocolCurtisRuiBoTe::_lift_flag = 0;

CNewProtocolCurtisRuiBoTe::CNewProtocolCurtisRuiBoTe()
{
	_encoderLine = 1024;
}

bool CNewProtocolCurtisRuiBoTe::encode(motorDriverCmdType ct, int32_t val)
{
	CanTxMsg msg;
	msg.StdId = 0x213;
	msg.IDE = CAN_Id_Standard;
	msg.RTR = CAN_RTR_Data;
	msg.DLC = 8;
	
	if (DRV_CMD_TARGET_SPEED == ct) {
		_speedVal = (int16_t)(val * 60.0f / (4.0f * _encoderLine));
		return false;
	} else if(DRV_CMD_TARGET_POS == ct) {
		uint8_t driveRawDir;
		uint16_t driveRawSpeed;
		uint16_t steerRawAngle;

		driveRawDir = (_speedVal < 0) ? 0x89: 0x85;   //forwoard ? down bit7 is 1
		driveRawSpeed = (_speedVal < 0) ? -_speedVal : _speedVal;

		steerRawAngle = - val * 10;
	
		msg.Data[0] = driveRawDir;
		msg.Data[1] = 0x00;
		//msg.Data[1] = driveRawSpeed & 0xFF;
		msg.Data[2] = 0x0a;			//acc
		msg.Data[3] = 0x0a;			//acc
		msg.Data[4] = steerRawAngle & 0xFF;
		msg.Data[5] = steerRawAngle >> 8;
		msg.Data[6] = driveRawSpeed & 0xFF;
		msg.Data[7] = driveRawSpeed >> 8;

		//msg.Data[5] = (uint8_t)((steerRawAngle) & 0xFF);	//theta L
		//msg.Data[6] = (steerRawAngle) >> 8;					//theta H
		//msg.Data[7] = GlobalControl::getBatterySoc() * 100;    //battery 
		_canRouter->putMsg(msg);



	} else if (DRV_CMD_FORK_VEL == ct) {
		_forkVel = val;
		if (_forkVel > 0) {
			_lift_flag = true;
			_lower_flag = false;
		} else if (_forkVel < 0) {
			_lift_flag = false;
			_lower_flag = true;
			_forkVel = -_forkVel;
		} else if (_forkVel == 0) {
			_lift_flag = false;
			_lower_flag = false;
		}

		msg.StdId = 0x313;
		msg.Data[0] = ((uint8_t)_lift_flag << 0) | ((uint8_t)_lower_flag << 1);
		if (_lift_flag)
			msg.Data[1] = (uint8_t)(_forkVel / 2.55f);
		else
			msg.Data[1] = 0x00;

		if (_lower_flag) {
			msg.Data[2] = (uint8_t)(_forkVel / 2.55f);
		} else {
			msg.Data[2] = 0x00;
		}
		msg.Data[3] = 0x00;							//theta L
		msg.Data[4] = 0x00;	
		msg.Data[5] = 0x00;							//theta L
		msg.Data[6] = 0x00;							//theta H
		msg.Data[7] = 0x00;

		_canRouter->putMsg(msg);

		return false;
	}
	return true;
}

int CNewProtocolCurtisRuiBoTe::decode(motorDriverCmdType (&ct)[10], int32_t (&val)[10], const CanRxMsg& canMsg)
{
	int16_t rawSpeed = 0;
	int16_t rawAngle = 0;
	uint8_t wheel_error = 0;
	uint8_t driver_stute = 0;
	
	memcpy(&wheel_error, &canMsg.Data[4], 1);

	uint8_t walk_error = 0;
	uint8_t steer_error = 0;
	uint8_t fork_error = 0;
	memcpy(&walk_error, &canMsg.Data[0],1);
	memcpy(&steer_error, &canMsg.Data[1],1);
	memcpy(&fork_error, &canMsg.Data[2],1);

	if (_motorType == 1 && canMsg.StdId == 0x207) {
		ct[0] = DRV_CMD_ACTURAL_SPEED;

		memcpy(&rawSpeed, &canMsg.Data[6], 2);
		val[0] = rawSpeed;

		return 1;
	} else if (_motorType == 2 && canMsg.StdId == 0x207) {
		ct[0] = DRV_CMD_ACTURAL_POS;

		memcpy(&rawAngle, &canMsg.Data[4], 2);
		val[0] = rawAngle / 10;

		return 1;
	} else if (_motorType == 6 && canMsg.StdId == 0x203) {
		ct[0] = DRV_CMD_AUTO_MODE;
		ct[1] = DRV_CMD_DRI_EMC;

		memcpy(&driver_stute, &canMsg.Data[0], 1);
		val[0] = (driver_stute & 0x02) ? 0 : 1;
		val[1] = (driver_stute & 0x10) ? 0 : 1;

		return 2;
	}  else if (_motorType == 6 && canMsg.StdId == 0x209) {
		ct[0] = DRV_CMD_DRI_ERR;
		ct[1] = DRV_CMD_DRI_ERRCODE;
		if(walk_error != 0 || steer_error != 0 || fork_error != 0){	
			val[0] = 1;
			uint32_t tempVal = 0;
			tempVal += ((uint32_t)canMsg.Data[0] << 24);
			tempVal += ((uint32_t)canMsg.Data[1] << 16);
			tempVal += ((uint32_t)canMsg.Data[1] << 8);
			val[1] = tempVal;
		}else{
			val[0] = 0;
			val[1] = 0;
		}
		//Console::Instance()->printf("data is 0x%02x,0x%02x,0x%02x,0x%02x,0x%02x,0x%02x,0x%02x,0x%02x\r\n", canMsg.Data[0],canMsg.Data[1],canMsg.Data[2],canMsg.Data[3],canMsg.Data[4],canMsg.Data[5],canMsg.Data[6],canMsg.Data[7]);
		return 2;
	} else {
		return 0;
	}
}
